#include "UWB_message_type.h"
#include <std_msgs/String.h>

void test_fun(std_msgs::String s)
{

    UWB_messages::EstimationMessage es_msg2 = UWB_messages::convertToMessage<UWB_messages::EstimationMessage>(s.data);
    std::cout <<es_msg2  << std::endl;

}

int main()
{
    // UWB_messages::ObservationMessage ob_msg;
    // std::cout << ob_msg << std::endl;
    UWB_messages::EstimationMessage es_msg;
    es_msg.systemTime = 10086;
    es_msg.pose.position = UWB_messages::Vector3(1,2,3);
    es_msg.pose.orientation = UWB_messages::Quaternion(4,5,6,7);
    es_msg.accel.linear = UWB_messages::Vector3(8,9,10);
    es_msg.accel.angular = UWB_messages::Vector3(11,12,13);
    std::cout << es_msg << std::endl;
    std_msgs::String s;

    s.data = UWB_messages::convertToString<UWB_messages::EstimationMessage>(es_msg);
    test_fun(s);
}
